/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Trip Queue
 *         $Id: tripq.c 7 2013-12-19 06:09:42Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "tripq.h"

#if 0
// Short Straight Line
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance        Wall
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {  300,  300 }, {   -1,   -1 } },
  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 3000, 3000 }, {   -1,   -1 } },
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {  300,  300 }, {   -1,   -1 } }
};
#endif

#if 0
// Straight Line
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance        Wall
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {  300,  300 }, {   -1,   -1 } },
  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 5000, 5000 }, {   -1,   -1 } },
  { FIXED_SPEED, 0, { 300, 300 }, { FORWARD, FORWARD }, {  400,  400 }, {   -1,   -1 } },
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {  300,  300 }, {   -1,   -1 } }
};
#endif

#if 1
// Long Straight Line
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance          Wall
  { FIXED_SPEED, 0, { 700, 700 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
};
#endif

#if 0
// Hallway Navigation
const tripq_t tripq[] = {
  //                  Speed                       Direction             Distance                     Wall
  { FIXED_SPEED,   0, { 200, 600 },             { FORWARD, FORWARD }, {  300,  300 },              {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 },             { FORWARD, FORWARD }, { 4200, 4200 },              {   -1,   -1 } },
  { TURNING,       0, { 600, in_whl_spd(600) }, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 },             { FORWARD, FORWARD }, { 3600 - (2 * TURN_LAT_DIST), 3600 - (2 * TURN_LAT_DIST) }, {   -1,   -1 } },
  { TURNING,       0, { in_whl_spd(600), 600 }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 },             { FORWARD, FORWARD }, { 2000, 2000 },              {   -1,   -1 } },
  { FIXED_SPEED,   0, { 400, 400 },             { FORWARD, FORWARD }, {  300,  300 },              {   -1,   -1 } },
  { FIXED_SPEED,   0, { 200, 200 },             { FORWARD, FORWARD }, {  200,  200 },              {   -1,   -1 } }
};
#endif

#if 0
// CW Square
const tripq_wheel_t tripq_left[] = {
  { 400, FORWARD, 1600 - TURN_LAT_DIST },
  { 400, FORWARD, ARC_OUTSIDE },
  { 400, FORWARD, 1600 - (2 * TURN_LAT_DIST) },
  { 400, FORWARD, ARC_OUTSIDE },
  { 400, FORWARD, 1600 - (2 * TURN_LAT_DIST) },
  { 400, FORWARD, ARC_OUTSIDE },
  { 400, FORWARD, 1300 - TURN_LAT_DIST },
  { 200, FORWARD, 300 },
};

const tripq_wheel_t tripq_right[] = {
  { 400, FORWARD, 1600 - TURN_LAT_DIST },
  { in_whl_spd(400), FORWARD, ARC_INSIDE },
  { 400, FORWARD, 1600 - (2 * TURN_LAT_DIST) },
  { in_whl_spd(400), FORWARD, ARC_INSIDE },
  { 400, FORWARD, 1600 - (2 * TURN_LAT_DIST) },
  { in_whl_spd(400), FORWARD, ARC_INSIDE },
  { 400, FORWARD, 1300 - TURN_LAT_DIST },
  { 200, FORWARD, 300 },
};
#endif

#if 0
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance        Wall
  { FIXED_SPEED,   0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { TURNING,       0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { TURNING,       0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { TURNING,       0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, {   -1,   -1 } }
};
#endif

#if 0
// Office Hallway
#define center_hall(width)   wall_dist(width), wall_dist(width)
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance        Wall
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {  300,  300 }, { center_hall(151) } },
  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 4660, 4660 }, { center_hall(151) } },
};
#endif

#if 0
// House Hallway
#define center_hall(width)   wall_dist(width), wall_dist(width)
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance        Wall
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {  300,  300 }, { center_hall(103) } },
  { FIXED_SPEED, 0, { 500, 500 }, { FORWARD, FORWARD }, { 4700, 4700 }, { center_hall(103) } },
};
#endif

#if 0
#define center_hall(width)   wall_dist(width), wall_dist(width)
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance          Wall
  { FIXED_SPEED, 0, { 200, 200 }, { FORWARD, FORWARD }, {   300,   300 }, { center_hall(105) } },
  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 20000, 20000 }, { center_hall(105) } },
};
#endif

#if 0

  
const tripq_t tripq[] = {
  { FIXED_SPEED,   0, { 200, 200 },             { FORWARD, FORWARD }, {  300,  300 },              {   -1,   -1 } },
  { FIXED_SPEED,   0, { 500, 500 },             { FORWARD, FORWARD }, { 5186, 5186 },              {   -1,   -1 } },
  { TURNING,       0, { 500, in_whl_spd(500) }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 500, 500 },             { FORWARD, FORWARD }, { 1524 - (2 * TURN_LAT_DIST), 1524 - (2 * TURN_LAT_DIST) }, {   -1,   -1 } },
  { TURNING,       0, { in_whl_spd(500), 500 }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 500, 500 },             { FORWARD, FORWARD }, { 2438 - (2 * TURN_LAT_DIST), 2438 - (2 * TURN_LAT_DIST) }, {   -1,   -1 } },
  { TURNING,       0, { in_whl_spd(500), 500 }, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, {   -1,   -1 } },
  { FIXED_SPEED,   0, { 500, 500 },             { FORWARD, FORWARD }, { 2438 - (2 * TURN_LAT_DIST), 2438 - (2 * TURN_LAT_DIST) }, {   -1,   -1 } },
};

#endif

const unsigned int tq_size = sizeof(tripq) / sizeof(tripq_t);
int tq_index;
